#include <chrono>
#include <memory>
#include <iostream>
#include <vector>
#include <math.h>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <sys/stat.h>   // 添加：stat结构体和函数
#include <cstdlib>      // 添加：system()函数
#include <thread>  // 用于std::this_thread::sleep_for

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include <custom_msgs/msg/gps_data_array.hpp>
#include "sensor_msgs/msg/nav_sat_fix.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "mavros_msgs/msg/override_rc_in.hpp"

#include "boat_datadef.h"
#include "motion_plan.h"
#include "motion_control.h"

#include <signal.h>
#include <csignal>

using namespace std::chrono_literals;

class USVController : public rclcpp::Node
{
public:
    USVController() : Node("usv_controller")
    {
        // 设置静态指针，用于信号处理
        instance_ = this;
        
        // 注册信号处理函数
        setupSignalHandlers();

        // 声明参数
        declareParameters();
        
        // 初始化CSV日志（在参数声明之后）
        initializeCSVLogging();

        // 获取参数
        loadParameters();
        
        // 初始化控制器
        initializeControllers();
        
        // 初始化状态
        initializeStates();
        
        // 创建ROS2接口
        createROSInterfaces();
        
        RCLCPP_INFO(this->get_logger(), 
            "USV Controller initialized (Type: %d, ID: %d, Freq: %.1f Hz)", 
            usv_type_, usv_id_, control_frequency_);
    }

    // 添加析构函数，确保节点停止时发送安全PWM值
    ~USVController()
    {
        // 清除静态指针
        instance_ = nullptr;

        // 关闭CSV文件
        if (csv_file_.is_open()) {
            csv_file_.close();
            RCLCPP_INFO(this->get_logger(), "CSV log file closed: %s", csv_filename_.c_str());
        }

        // 发送安全停止PWM信号
        if (pwm_pub_) {
            mavros_msgs::msg::OverrideRCIn safe_stop_msg;
            std::fill(safe_stop_msg.channels.begin(), safe_stop_msg.channels.end(), 65535);
            
            safe_stop_msg.channels[2] = 1500;  // 电机停止
            safe_stop_msg.channels[3] = 1500;  // 舵机中立
            
            pwm_pub_->publish(safe_stop_msg);
            
            // 等待消息发送完成
            rclcpp::sleep_for(std::chrono::milliseconds(100));
            
            RCLCPP_INFO(this->get_logger(), "Safe stop PWM signals sent on node destruction");
        }
    }

private:
    void declareParameters()
    {
        // 基本参数
        this->declare_parameter("usv_type", 2);
        this->declare_parameter("usv_id", 0);
        this->declare_parameter("control_frequency", 10.0);
        
        // 轨迹跟踪参数
        this->declare_parameter("tracking.eta", 0.8);
        this->declare_parameter("tracking.switch_distance", 15.0);
        this->declare_parameter("tracking.max_speed", 5.0);
        this->declare_parameter("tracking.default_speed", 2.0);
        this->declare_parameter("tracking.min_speed_factor", 0.2);
        
        // 航向PID参数
        this->declare_parameter("heading_pid.kp", 0.35);
        this->declare_parameter("heading_pid.ki", 0.0);
        this->declare_parameter("heading_pid.kd", 3.0);
        this->declare_parameter("heading_pid.max_integral", 2000.0);
        this->declare_parameter("heading_pid.min_output", -30.0);
        this->declare_parameter("heading_pid.max_output", 30.0);
        
        // 速度PID参数
        this->declare_parameter("velocity_pid.kp", 200.0);
        this->declare_parameter("velocity_pid.ki", 1.0);
        this->declare_parameter("velocity_pid.kd", 0.0);
        this->declare_parameter("velocity_pid.max_integral", 2000.0);
        this->declare_parameter("velocity_pid.min_output", 10.0);
        this->declare_parameter("velocity_pid.max_output", 1000.0);
        
        // 话题配置
        this->declare_parameter("topics.target_position", "/target_global_position");
        this->declare_parameter("topics.usv_state", "/usv_state_1");
        this->declare_parameter("topics.usv_control", "/usv_control_1");
        this->declare_parameter("topics.usv_task", "/usv_task_1");  // 新增
        
        // 新增舵机控制参数
        this->declare_parameter("control.steering_deadzone", 1.5);
        this->declare_parameter("control.steering_filter_alpha", 0.75);
        // 新增PWM平滑控制参数
        this->declare_parameter("control.pwm_smooth_steps", 5);
        this->declare_parameter("control.step_delay_factor", 0.85);
        
        // 安全参数
        this->declare_parameter("safety.max_heading_error", 30.0);
        this->declare_parameter("safety.speed_reduction_angle", 30.0);
        this->declare_parameter("safety.approach_distance", 30.0);
        
        // 调试参数
        this->declare_parameter("debug.enable_debug_output", false);
        this->declare_parameter("debug.state_print_interval", 50);
        this->declare_parameter("debug.control_print_interval", 20);
    }
    
    void loadParameters()
    {
        // 基本参数
        usv_type_ = this->get_parameter("usv_type").as_int();
        usv_id_ = this->get_parameter("usv_id").as_int();
        control_frequency_ = this->get_parameter("control_frequency").as_double();
        
        // 限制控制频率
        if (control_frequency_ > 15.0) {
            RCLCPP_WARN(this->get_logger(), "Control frequency too high for stable servo control, limiting to 15Hz");
            control_frequency_ = 15.0;
        }
        
        // 轨迹跟踪参数
        tracking_params_.eta = this->get_parameter("tracking.eta").as_double();
        tracking_params_.switch_distance = this->get_parameter("tracking.switch_distance").as_double();
        tracking_params_.max_speed = this->get_parameter("tracking.max_speed").as_double();
        tracking_params_.default_speed = this->get_parameter("tracking.default_speed").as_double();
        tracking_params_.min_speed_factor = this->get_parameter("tracking.min_speed_factor").as_double();
        
        // PID参数
        heading_pid_params_.kp = this->get_parameter("heading_pid.kp").as_double();
        heading_pid_params_.ki = this->get_parameter("heading_pid.ki").as_double();
        heading_pid_params_.kd = this->get_parameter("heading_pid.kd").as_double();
        heading_pid_params_.max_integral = this->get_parameter("heading_pid.max_integral").as_double();
        heading_pid_params_.min_output = this->get_parameter("heading_pid.min_output").as_double();
        heading_pid_params_.max_output = this->get_parameter("heading_pid.max_output").as_double();
        
        velocity_pid_params_.kp = this->get_parameter("velocity_pid.kp").as_double();
        velocity_pid_params_.ki = this->get_parameter("velocity_pid.ki").as_double();
        velocity_pid_params_.kd = this->get_parameter("velocity_pid.kd").as_double();
        velocity_pid_params_.max_integral = this->get_parameter("velocity_pid.max_integral").as_double();
        velocity_pid_params_.min_output = this->get_parameter("velocity_pid.min_output").as_double();
        velocity_pid_params_.max_output = this->get_parameter("velocity_pid.max_output").as_double();
        
        // 话题名称
        topic_target_position_ = this->get_parameter("topics.target_position").as_string();
        topic_usv_state_ = this->get_parameter("topics.usv_state").as_string();
        topic_usv_control_ = this->get_parameter("topics.usv_control").as_string();
        topic_usv_task_ = this->get_parameter("topics.usv_task").as_string();  // 新增

        // 加载舵机控制参数
        steering_deadzone_ = this->get_parameter("control.steering_deadzone").as_double();
        steering_filter_alpha_ = this->get_parameter("control.steering_filter_alpha").as_double();
        // 加载PWM平滑控制参数
        pwm_smooth_steps_ = this->get_parameter("control.pwm_smooth_steps").as_int();
        pwm_smooth_steps_ = std::max(1, std::min(10, pwm_smooth_steps_));
        double step_delay_factor = this->get_parameter("control.step_delay_factor").as_double();
        step_delay_factor = std::max(0.5, std::min(0.95, step_delay_factor));
        
        // 计算延时时间
        cycle_time_ms = 1000.0 / control_frequency_;
        double theoretical_step_time_ms = cycle_time_ms / pwm_smooth_steps_;
        double actual_step_time_ms = theoretical_step_time_ms * step_delay_factor;
        step_delay_microseconds_ = static_cast<int>(actual_step_time_ms * 1000.0);

        RCLCPP_INFO(this->get_logger(), 
            "PWM Smooth Control: steps=%d, cycle_time=%.1fms, step_delay=%.1fms", 
            pwm_smooth_steps_, cycle_time_ms, actual_step_time_ms);

        // 安全参数
        safety_params_.max_heading_error = this->get_parameter("safety.max_heading_error").as_double();
        safety_params_.speed_reduction_angle = this->get_parameter("safety.speed_reduction_angle").as_double();
        safety_params_.approach_distance = this->get_parameter("safety.approach_distance").as_double();
        
        // 调试参数
        debug_enabled_ = this->get_parameter("debug.enable_debug_output").as_bool();
        state_print_interval_ = this->get_parameter("debug.state_print_interval").as_int();
        control_print_interval_ = this->get_parameter("debug.control_print_interval").as_int();
    }
    
    void initializeControllers()
    {
        motion_planner_ = std::make_unique<MotionPlan>();
        motion_controller_ = std::make_unique<MotionControl>();
        
        // 设置控制器类型
        motion_controller_->SetController(usv_type_);
        
        // 配置PID控制器
        motion_controller_->SetHeadingPID(
            static_cast<float>(heading_pid_params_.kp),
            static_cast<float>(heading_pid_params_.ki),
            static_cast<float>(heading_pid_params_.kd),
            static_cast<float>(heading_pid_params_.max_integral),
            static_cast<float>(heading_pid_params_.min_output),
            static_cast<float>(heading_pid_params_.max_output)
        );
        
        motion_controller_->SetVelocityPID(
            static_cast<float>(velocity_pid_params_.kp),
            static_cast<float>(velocity_pid_params_.ki),
            static_cast<float>(velocity_pid_params_.kd),
            static_cast<float>(velocity_pid_params_.max_integral),
            static_cast<float>(velocity_pid_params_.min_output),
            static_cast<float>(velocity_pid_params_.max_output)
        );
        
        // 设置运动规划参数
        motion_planner_->SetTrackingParameters(
            static_cast<float>(tracking_params_.eta),
            static_cast<float>(tracking_params_.switch_distance),
            static_cast<float>(tracking_params_.max_speed),
            static_cast<float>(tracking_params_.default_speed),
            static_cast<float>(tracking_params_.min_speed_factor)
        );
        
        RCLCPP_INFO(this->get_logger(), "Controllers initialized successfully");
    }
    
    void initializeStates()
    {
        memset(&current_state_, 0, sizeof(kinematic_state));
        memset(&trans_state_, 0, sizeof(TransState));
        memset(&mission_data_, 0, sizeof(mission_control));
        
        current_state_.kind = static_cast<uint8_t>(usv_type_);
        current_state_.num = static_cast<uint8_t>(usv_id_);
        
        // 初始化变换状态
        for (int i = 0; i < 4; i++) {
            trans_state_.HomePos[i] = 0.0;
            trans_state_.PosEarth[i] = 0.0;
            trans_state_.PostoHome[i] = 0.0;
            trans_state_.VelEarth[i] = 0.0f;
            trans_state_.VelPolar[i] = 0.0f;
            trans_state_.VelBody[i] = 0.0f;
        }
        
        // 初始化飞控数据状态标志
        velocity_data_received_ = false;
        imu_data_received_ = false;
        gps_data_received_ = false;

        // 初始化舵机控制相关变量
        last_steering_command_ = 0.0f;
        last_significant_change_time_ = this->now();
        
        last_control_time_ = this->now();
    }
    
    void createROSInterfaces()
    {
        // 创建兼容mavros的QoS设置
        auto sensor_qos = rclcpp::QoS(10)
            .reliability(rclcpp::ReliabilityPolicy::BestEffort)
            .durability(rclcpp::DurabilityPolicy::Volatile)
            .history(rclcpp::HistoryPolicy::KeepLast);
            
        // 创建订阅者 - 目标点
        target_pos_sub_ = this->create_subscription<custom_msgs::msg::GpsDataArray>(
            topic_target_position_, 10,
            std::bind(&USVController::targetPositionCallback, this, std::placeholders::_1));
        
        // 创建订阅者 - 原始状态数据（保留，以防需要）
        state_sub_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
            topic_usv_state_, 10,
            std::bind(&USVController::stateCallback, this, std::placeholders::_1));
        
        // 新增：创建三个飞控数据订阅者
        velocity_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            "/mavros/local_position/velocity_local", sensor_qos,
            std::bind(&USVController::velocityCallback, this, std::placeholders::_1));
            
        imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "/mavros/imu/data", sensor_qos,
            std::bind(&USVController::imuCallback, this, std::placeholders::_1));
            
        gps_sub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            "/mavros/global_position/global", sensor_qos,
            std::bind(&USVController::gpsCallback, this, std::placeholders::_1));
        
        // 创建发布者
        control_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(topic_usv_control_, 10);

        // 新增：创建任务发布者
        task_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(topic_usv_task_, 10);
        
        // 新增：创建状态数据发布者
        usv_state_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(topic_usv_state_, 10);

        // 新增：创建PWM发布者
        pwm_pub_ = this->create_publisher<mavros_msgs::msg::OverrideRCIn>("/mavros/rc/override", 10);
        
        // 创建定时器
        auto timer_period = std::chrono::milliseconds(static_cast<int>(cycle_time_ms));
        control_timer_ = this->create_wall_timer(timer_period,
            std::bind(&USVController::controlLoop, this));
    }
    
    // 新增：三轴线速度和三轴角速度回调函数
    void velocityCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
    {
        velocity_data_.linear_x = msg->twist.linear.x;
        velocity_data_.linear_y = msg->twist.linear.y;
        velocity_data_.linear_z = msg->twist.linear.z;
        velocity_data_.angular_x = msg->twist.angular.x;
        velocity_data_.angular_y = msg->twist.angular.y;
        velocity_data_.angular_z = msg->twist.angular.z;
        velocity_data_received_ = true;
        
        if (debug_enabled_) {
            RCLCPP_DEBUG(this->get_logger(), 
                "Velocity received: linear=[%.2f, %.2f, %.2f], angular=[%.2f, %.2f, %.2f]",
                velocity_data_.linear_x, velocity_data_.linear_y, velocity_data_.linear_z,
                velocity_data_.angular_x, velocity_data_.angular_y, velocity_data_.angular_z);
        }
    }
    
    // 新增：三轴姿态角和三轴线加速度回调函数
    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        // 存储四元数（用于计算欧拉角）
        imu_data_.orientation_x = msg->orientation.x;
        imu_data_.orientation_y = msg->orientation.y;
        imu_data_.orientation_z = msg->orientation.z;
        imu_data_.orientation_w = msg->orientation.w;
        
        // 转换四元数到欧拉角 (roll, pitch, yaw)
        convertQuaternionToEuler(msg->orientation.x, msg->orientation.y, 
                                msg->orientation.z, msg->orientation.w,
                                imu_data_.roll, imu_data_.pitch, imu_data_.yaw);
        
        // 存储线加速度
        imu_data_.linear_acceleration_x = msg->linear_acceleration.x;
        imu_data_.linear_acceleration_y = msg->linear_acceleration.y;
        imu_data_.linear_acceleration_z = msg->linear_acceleration.z;
        
        // 存储角速度
        imu_data_.angular_velocity_x = msg->angular_velocity.x;
        imu_data_.angular_velocity_y = msg->angular_velocity.y;
        imu_data_.angular_velocity_z = msg->angular_velocity.z;
        
        imu_data_received_ = true;
        
        if (debug_enabled_) {
            RCLCPP_DEBUG(this->get_logger(), 
                "IMU received: rpy=[%.1f, %.1f, %.1f], acc=[%.2f, %.2f, %.2f]",
                imu_data_.roll * 180.0 / M_PI, imu_data_.pitch * 180.0 / M_PI, imu_data_.yaw * 180.0 / M_PI,
                imu_data_.linear_acceleration_x, imu_data_.linear_acceleration_y, imu_data_.linear_acceleration_z);
        }
    }
    
    // 新增：经纬度和海拔回调函数
    void gpsCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg)
    {
        // 检查GPS数据有效性
        if (std::isnan(msg->latitude) || std::isnan(msg->longitude)) {
            RCLCPP_WARN(this->get_logger(), "Invalid GPS data received");
            return;
        }
        
        gps_data_.latitude = msg->latitude;
        gps_data_.longitude = msg->longitude;
        gps_data_.altitude = std::isnan(msg->altitude) ? 0.0 : msg->altitude;
        gps_data_received_ = true;
        
        if (debug_enabled_) {
            RCLCPP_DEBUG(this->get_logger(), 
                "GPS received: lat=%.6f, lon=%.6f, alt=%.2f",
                gps_data_.latitude, gps_data_.longitude, gps_data_.altitude);
        }
        
        // 检测数据完整性并发布合并后的状态
        checkDataCompletenessAndPublish();
    }
    
    // 新增：检测数据完整性并发布合并状态
    void checkDataCompletenessAndPublish()
    {
        // 检查所有数据是否都已接收
        if (!velocity_data_received_ || !imu_data_received_ || !gps_data_received_) {
            if (debug_enabled_) {
                RCLCPP_DEBUG(this->get_logger(), 
                    "Data incomplete: velocity=%d, imu=%d, gps=%d",
                    velocity_data_received_, imu_data_received_, gps_data_received_);
            }
            return;
        }
        
        // 组合成std_msgs::msg::Float64MultiArray消息
        std_msgs::msg::Float64MultiArray usv_state_msg;
        usv_state_msg.data.clear();
        usv_state_msg.data.reserve(17); // 预分配空间
        
        // 按照原始格式填充数据
        usv_state_msg.data.push_back(static_cast<double>(usv_type_));        // [0] kind
        usv_state_msg.data.push_back(static_cast<double>(usv_id_));          // [1] num
        usv_state_msg.data.push_back(gps_data_.latitude);                    // [2] lat
        usv_state_msg.data.push_back(gps_data_.longitude);                   // [3] lon
        usv_state_msg.data.push_back(gps_data_.altitude);                    // [4] alt (复用为航向角)
        usv_state_msg.data.push_back(imu_data_.roll * 180.0 / M_PI);         // [5] roll (转为度)
        usv_state_msg.data.push_back(imu_data_.pitch * 180.0 / M_PI);        // [6] pitch (转为度)
        usv_state_msg.data.push_back(imu_data_.yaw * 180.0 / M_PI);          // [7] yaw (转为度)
        usv_state_msg.data.push_back(imu_data_.angular_velocity_x * 180.0 / M_PI); // [8] rollspeed (转为度/s)
        usv_state_msg.data.push_back(imu_data_.angular_velocity_y * 180.0 / M_PI); // [9] pitchspeed (转为度/s)
        usv_state_msg.data.push_back(imu_data_.angular_velocity_z * 180.0 / M_PI); // [10] yawspeed (转为度/s)
        usv_state_msg.data.push_back(velocity_data_.linear_x);               // [11] vx
        usv_state_msg.data.push_back(velocity_data_.linear_y);               // [12] vy
        usv_state_msg.data.push_back(velocity_data_.linear_z);               // [13] vz
        usv_state_msg.data.push_back(imu_data_.linear_acceleration_x);       // [14] xacc
        usv_state_msg.data.push_back(imu_data_.linear_acceleration_y);       // [15] yacc
        usv_state_msg.data.push_back(imu_data_.linear_acceleration_z);       // [16] zacc
        
        // 发布到/usv_state话题
        usv_state_pub_->publish(usv_state_msg);
        
        if (debug_enabled_) {
            static int publish_count = 0;
            if (++publish_count % 50 == 0) { // 每50次打印一次
                RCLCPP_INFO(this->get_logger(), 
                    "Published USV state: lat=%.6f, lon=%.6f, yaw=%.1f°, speed=%.2f m/s",
                    gps_data_.latitude, gps_data_.longitude, 
                    imu_data_.yaw * 180.0 / M_PI,
                    sqrt(velocity_data_.linear_x * velocity_data_.linear_x + 
                         velocity_data_.linear_y * velocity_data_.linear_y));
            }
        }
    }
    
    // 新增：四元数转欧拉角函数
    void convertQuaternionToEuler(double qx, double qy, double qz, double qw,
                              double& roll, double& pitch, double& yaw)
    {
        // 先正常计算ENU坐标系的欧拉角
        double sinr_cosp = 2 * (qw * qx + qy * qz);
        double cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
        roll = atan2(sinr_cosp, cosr_cosp);
        
        double sinp = 2 * (qw * qy - qz * qx);
        if (abs(sinp) >= 1)
            pitch = copysign(M_PI / 2, sinp);
        else
            pitch = asin(sinp);
            
        double siny_cosp = 2 * (qw * qz + qx * qy);
        double cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
        double yaw_enu = atan2(siny_cosp, cosy_cosp);
        
        // 然后转换为NED坐标系（北东地）的航向角
        yaw = M_PI/2 - yaw_enu;  // 相当于 90° - yaw_enu
        if (yaw < 0) {
            yaw += 2 * M_PI;
        }
    }
    
    void targetPositionCallback(const custom_msgs::msg::GpsDataArray::SharedPtr msg)
    {
        if (msg->gps_data.empty()) {
            RCLCPP_WARN(this->get_logger(), "Received empty GPS data array");
            return;
        }
        
        int added_count = 0;
        
        // 处理数组中的每个GPS点
        for (const auto& gps_point : msg->gps_data) {
            // 验证GPS数据有效性
            if (!isValidGPSData(gps_point)) {
                RCLCPP_WARN(this->get_logger(), "Invalid GPS coordinates received: lat=%.6f, lon=%.6f", 
                        gps_point.latitude, gps_point.longitude);
                continue;
            }
            
            // 添加航点到任务队列
            if (addWaypointToMission(gps_point)) {
                added_count++;
                RCLCPP_INFO(this->get_logger(), 
                    "Added waypoint %d: lat=%.6f, lon=%.6f, speed=%.1f m/s", 
                    mission_data_.mission_num, gps_point.latitude, gps_point.longitude, 
                    tracking_params_.default_speed);
            }
        }
        
        if (added_count > 0) {
            RCLCPP_INFO(this->get_logger(), "Added %d new waypoints to mission", added_count);

            // 新增：发布任务数据到/usv_task_1话题
            publishMissionTask();
            
            // 如果这是第一批目标点且状态已接收，开始任务
            if (!mission_active_ && state_received_ && mission_data_.mission_num > 0) {
                startMission();
            }
        }
    }
    
    bool isValidGPSData(const sensor_msgs::msg::NavSatFix& gps_msg)
    {
        return !std::isnan(gps_msg.latitude) && !std::isnan(gps_msg.longitude) &&
            gps_msg.latitude >= -90.0 && gps_msg.latitude <= 90.0 &&
            gps_msg.longitude >= -180.0 && gps_msg.longitude <= 180.0;
    }
    
    bool addWaypointToMission(const sensor_msgs::msg::NavSatFix& gps_msg)
    {
        if (mission_data_.mission_num >= 128) {
            RCLCPP_WARN(this->get_logger(), "Mission queue full, ignoring new target position");
            return false;
        }
        
        MissionItem new_mission;
        memset(&new_mission, 0, sizeof(MissionItem));
        
        new_mission.command = 16;  // 航点命令
        new_mission.seq = mission_data_.mission_num;
        new_mission.param1 = static_cast<float>(tracking_params_.default_speed);
        new_mission.param2 = 0.0f;  // 等待时间
        new_mission.lat = static_cast<float>(gps_msg.latitude);
        new_mission.lon = static_cast<float>(gps_msg.longitude);
        new_mission.alt = static_cast<float>(gps_msg.altitude);
        
        mission_data_.mission[mission_data_.mission_num] = new_mission;
        mission_data_.mission_num++;
        
        return true;
    }
    
    void publishMissionTask()
    {
        if (mission_data_.mission_num == 0) {
            return;
        }
        
        std_msgs::msg::Float64MultiArray task_msg;
        task_msg.data.clear();
        
        // 按照表格格式组织数据
        // data[0]: 任务指令总数 (固定为20，表示最多20个任务点)
        // data[1]: mission_num (实际任务点数量)
        task_msg.data.push_back(20.0);  // 固定值20
        task_msg.data.push_back(static_cast<double>(mission_data_.mission_num));
        
        // 为每个任务点添加10个参数 (从data[2]开始)
        for (int i = 0; i < mission_data_.mission_num && i < 18; i++) {  // 最多18个点 (2 + 18*10 = 182 < 200)
            const MissionItem& item = mission_data_.mission[i];
            
            task_msg.data.push_back(static_cast<double>(item.command));  // command (固定为16)
            task_msg.data.push_back(static_cast<double>(item.seq));      // seq
            task_msg.data.push_back(static_cast<double>(item.param1));   // 速度 x
            task_msg.data.push_back(static_cast<double>(item.param2));   // 速度 y  
            task_msg.data.push_back(static_cast<double>(item.param3));   // 速度 z
            task_msg.data.push_back(static_cast<double>(item.param1));   // param1 (保留)
            task_msg.data.push_back(static_cast<double>(item.param2));   // param2 (航速)
            task_msg.data.push_back(static_cast<double>(item.param3));   // param3 (保留)
            task_msg.data.push_back(static_cast<double>(item.param4));   // param4 (保留)
            task_msg.data.push_back(static_cast<double>(item.lat));      // 纬度
            task_msg.data.push_back(static_cast<double>(item.lon));      // 经度
        }
        
        // 发布任务
        task_pub_->publish(task_msg);
        
        RCLCPP_INFO(this->get_logger(), 
            "Published mission task with %d waypoints to topic '%s'", 
            mission_data_.mission_num, topic_usv_task_.c_str());
    }

    void stateCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
    {
        if (msg->data.size() < 17) {
            RCLCPP_WARN(this->get_logger(), "Invalid state message size: %zu, expected >= 17", msg->data.size());
            return;
        }
        
        // 更新当前状态
        updateKinematicState(msg);
        
        // 更新变换状态
        updateTransState();
        
        if (!state_received_) {
            state_received_ = true;
            RCLCPP_INFO(this->get_logger(), "First state received, controller ready");
        }
        
        // 调试输出
        printStateDebugInfo();
    }
    
    void updateKinematicState(const std_msgs::msg::Float64MultiArray::SharedPtr& msg)
    {
        current_state_.kind = static_cast<uint8_t>(msg->data[0]);
        current_state_.num = static_cast<uint8_t>(msg->data[1]);
        current_state_.lat = msg->data[2];
        current_state_.lon = msg->data[3];
        current_state_.alt = msg->data[4];
        current_state_.roll = static_cast<float>(msg->data[5]);
        current_state_.pitch = static_cast<float>(msg->data[6]);
        current_state_.yaw = static_cast<float>(msg->data[7]);
        current_state_.rollspeed = static_cast<float>(msg->data[8]);
        current_state_.pitchspeed = static_cast<float>(msg->data[9]);
        current_state_.yawspeed = static_cast<float>(msg->data[10]);
        current_state_.vx = static_cast<float>(msg->data[11]);
        current_state_.vy = static_cast<float>(msg->data[12]);
        current_state_.vz = static_cast<float>(msg->data[13]);
        current_state_.xacc = static_cast<float>(msg->data[14]);
        current_state_.yacc = static_cast<float>(msg->data[15]);
        current_state_.zacc = static_cast<float>(msg->data[16]);
    }
    
    void updateTransState()
    {
        // 设置原点 (首次接收状态时)
        if (trans_state_.HomePos[0] == 0.0 && trans_state_.HomePos[1] == 0.0) {
            trans_state_.HomePos[0] = current_state_.lat;
            trans_state_.HomePos[1] = current_state_.lon;
            trans_state_.HomePos[2] = current_state_.alt;
            trans_state_.HomePos[3] = current_state_.yaw;
        }
        
        // 更新地球坐标位置
        trans_state_.PosEarth[0] = current_state_.lat;
        trans_state_.PosEarth[1] = current_state_.lon;
        trans_state_.PosEarth[2] = current_state_.alt;
        trans_state_.PosEarth[3] = current_state_.yaw;
        
        // 更新相对位置
        trans_state_.PostoHome[0] = current_state_.lat - trans_state_.HomePos[0];
        trans_state_.PostoHome[1] = current_state_.lon - trans_state_.HomePos[1];
        trans_state_.PostoHome[2] = current_state_.alt - trans_state_.HomePos[2];
        trans_state_.PostoHome[3] = current_state_.yaw - trans_state_.HomePos[3];
        
        // 更新速度信息
        trans_state_.VelEarth[0] = current_state_.vx;
        trans_state_.VelEarth[1] = current_state_.vy;
        trans_state_.VelEarth[2] = current_state_.vz;
        trans_state_.VelEarth[3] = current_state_.yawspeed;
        
        // 计算极坐标速度
        float speed = sqrtf(current_state_.vx * current_state_.vx + current_state_.vy * current_state_.vy);
        float course = atan2f(current_state_.vx, current_state_.vy) * 180.0f / M_PI;
        if (course < 0) course += 360.0f;
        
        trans_state_.VelPolar[0] = speed;
        trans_state_.VelPolar[1] = course;
        trans_state_.VelPolar[2] = current_state_.vz;
        trans_state_.VelPolar[3] = current_state_.yawspeed;
        
        // 载体坐标速度 (简化处理)
        trans_state_.VelBody[0] = current_state_.vx;
        trans_state_.VelBody[1] = current_state_.vy;
        trans_state_.VelBody[2] = current_state_.vz;
        trans_state_.VelBody[3] = current_state_.yawspeed;
    }
    
    void printStateDebugInfo()
    {
        if (debug_enabled_ && (++state_count_ % state_print_interval_ == 0)) {
            float speed = sqrtf(current_state_.vx * current_state_.vx + current_state_.vy * current_state_.vy);
            RCLCPP_DEBUG(this->get_logger(), 
                "State: lat=%.6f, lon=%.6f, yaw=%.1f°, speed=%.2f m/s",
                current_state_.lat, current_state_.lon, current_state_.yaw, speed);
        }
    }
    
    void startMission()
    {
        if (mission_data_.mission_num > 0 && state_received_) {
            if (motion_planner_->TrajectoryGet(mission_data_)) {
                mission_active_ = true;
                RCLCPP_INFO(this->get_logger(), "Mission started with %d waypoints", mission_data_.mission_num);
            } else {
                RCLCPP_ERROR(this->get_logger(), "Failed to start mission");
            }
        }
    }
    
    void controlLoop()
    {
        if (!state_received_ || !mission_active_) {
            return;
        }
        
        // 计算控制周期
        auto current_time = this->now();
        float dt = (current_time - last_control_time_).seconds();
        last_control_time_ = current_time;
        
        // 限制dt在合理范围内
        dt = std::max(0.01f, std::min(0.5f, dt));
        
        // 执行轨迹跟踪
        motion_planner_->TrajectoryTrackingDirect(trans_state_);
        
        // 生成控制指令
        publishControlCommands(dt);
        
        // 检查任务完成状态
        if (motion_planner_->IsTrackingComplete()) {
            RCLCPP_INFO(this->get_logger(), "Mission completed!");
            mission_active_ = false;
            publishStopCommands();
        }
    }
    
    void publishControlCommands(float dt)
    {
        if (motion_planner_->actionModelData.model == 10) {
            // 航向速度控制模式
            actuator_control control_output = motion_controller_->USVControl(
                motion_planner_->actionHeadvelControl, trans_state_, dt);
            
            // 发布详细控制指令
            publishActuatorControl(control_output);
            
            // 记录CSV数据
            logCSVData(control_output);
            
            // 调试信息
            printControlDebugInfo(control_output);
        }
    }

    // PWM值映射函数
    uint16_t mapSteerToPWM(float steer_value)
    {
        // 将-30~30映射到1000-2000
        // 1. 限制最大舵角
        steer_value = std::max(-30.0f, std::min(30.0f, steer_value));
        
        // 线性映射：y = mx + b
        // 当steer_value = -30时，PWM = 1100
        // 当steer_value = 30时，PWM = 1900
        // m = (1900-1100)/(30-(-30)) = 800/60 = 13.33
        // b = 1100 - (-30) * 13.33 = 1500
        
        // 2. 应用死区控制 - 避免微小变化导致的抖动
        float steering_change = steer_value - last_steering_command_;
        if (fabs(steering_change) < steering_deadzone_) {
            // 在死区内，使用上次指令值
            steer_value = last_steering_command_;
        } else {
            // 超出死区，更新指令值
            last_steering_command_ = steer_value;
        }
        
        // 4. PWM映射：±30度映射到PWM值
        uint16_t pwm_value = static_cast<uint16_t>(steer_value * 13.33f + 1500.0f);
        return std::max(static_cast<uint16_t>(1100), std::min(static_cast<uint16_t>(1900), pwm_value));
    }
        
    uint16_t mapMotorToPWM(float motor_value)
    {
        // 将0-1000映射到1500-1900
        // 限制输入范围
        motor_value = std::max(0.0f, std::min(1000.0f, motor_value));
        
        // 线性映射
        // 当motor_value = 0时，PWM = 1500
        // 当motor_value = 1000时，PWM = 1900
        // m = (1900-1500)/(1000-0) = 400/1000 = 0.4
        // b = 1500
        
        uint16_t pwm_value = static_cast<uint16_t>(motor_value * 0.4f + 1500.0f);
        return std::max(static_cast<uint16_t>(1500), std::min(static_cast<uint16_t>(1900), pwm_value));
    }
    
    void publishActuatorControl(const actuator_control& control)
    {
        std_msgs::msg::Float64MultiArray control_msg;
        control_msg.data.clear();
        control_msg.data.push_back(control.steer_l);
        control_msg.data.push_back(control.steer_r);
        control_msg.data.push_back(control.motor_l);
        control_msg.data.push_back(control.motor_r);
        control_msg.data.push_back(control.bucket_l);
        control_msg.data.push_back(control.bucket_r);
        
        control_pub_->publish(control_msg);

        // 新增：发布PWM消息
        publishPWMCommands(control);
    }

    // 添加PWM发布函数
    void publishPWMCommands(const actuator_control& control)
    {
        // 计算目标PWM值
        uint16_t target_steer_pwm = mapSteerToPWM(control.steer_l);
        uint16_t target_motor_pwm = mapMotorToPWM(control.motor_l);
        
        // 使用多步平滑过渡
        applySmoothPWMSteps(target_steer_pwm, target_motor_pwm);
        
        // 降低调试输出频率
        if (debug_enabled_ && (control_count_ % 50 == 0)) {
            RCLCPP_INFO(this->get_logger(),
                "PWM: Motor=%d->%d, Steer=%d->%d (cmd: %.1f°)", 
                current_motor_pwm_, target_motor_pwm,
                current_steer_pwm_, target_steer_pwm,
                control.steer_l);
        }
    }
    
    // 新增：多步PWM平滑函数
    void applySmoothPWMSteps(uint16_t target_steer_pwm, uint16_t target_motor_pwm)
    {
        // 执行多步平滑过渡
        for (int step = 0; step < pwm_smooth_steps_; step++) {
            // 计算剩余步数
            int remaining_steps = pwm_smooth_steps_ - step;
            
            // 平滑舵机PWM
            current_steer_pwm_ = smoothPWMTransition(current_steer_pwm_, target_steer_pwm, remaining_steps);
            
            // 平滑电机PWM  
            current_motor_pwm_ = smoothPWMTransition(current_motor_pwm_, target_motor_pwm, remaining_steps);
            
            // 发布当前步骤的PWM值
            publishSinglePWMCommand();
            
            // 如果已经到达目标，提前退出
            if (current_steer_pwm_ == target_steer_pwm && current_motor_pwm_ == target_motor_pwm) {
                break;
            }
            
            // 步骤间的小延时
            std::this_thread::sleep_for(std::chrono::microseconds(step_delay_microseconds_));
        }
    }
    
    uint16_t smoothPWMTransition(uint16_t current_pwm, uint16_t target_pwm, int steps)
    {
        if (steps <= 1) {
            return target_pwm;  // 如果步数为1或更少，直接返回目标值
        }
        
        int16_t total_difference = static_cast<int16_t>(target_pwm) - static_cast<int16_t>(current_pwm);
        
        // 如果差值很小，直接到达目标
        if (abs(total_difference) <= 3) {
            return target_pwm;
        }
        
        // 计算每步的变化量
        int16_t step_change = total_difference / steps;
        
        // 确保至少有1的变化（避免步长为0导致无法到达目标）
        if (step_change == 0 && total_difference != 0) {
            step_change = (total_difference > 0) ? 1 : -1;
        }
        
        return static_cast<uint16_t>(current_pwm + step_change);
    }
    
    // 新增：发布单次PWM指令的函数
    void publishSinglePWMCommand()
    {
        mavros_msgs::msg::OverrideRCIn pwm_msg;
        std::fill(pwm_msg.channels.begin(), pwm_msg.channels.end(), 65535);
        
        pwm_msg.channels[2] = current_motor_pwm_;  // 电机通道
        pwm_msg.channels[3] = current_steer_pwm_;  // 舵机通道
        
        pwm_pub_->publish(pwm_msg);
    }
    
    void printControlDebugInfo(const actuator_control& control)
    {
        if (debug_enabled_ && (++control_count_ % control_print_interval_ == 0)) {
            float distance = motion_planner_->GetDistanceToTarget(trans_state_);
            RCLCPP_INFO(this->get_logger(),
                "Control: WP %d/%d, dist=%.1fm, heading=%.1f°, velocity=%.2f m/s, steer=%.1f°, motor_l=%.0f, motor_r=%.0f",
                motion_planner_->GetCurrentWaypointIndex() + 1,
                motion_planner_->GetTotalWaypoints(),
                distance,
                motion_planner_->actionHeadvelControl.heading,
                motion_planner_->actionHeadvelControl.velocity,
                control.steer_l, control.motor_l, control.motor_r);
        }
    }
    
    void publishStopCommands()
    {
        // 发送停止指令到控制话题
        std_msgs::msg::Float64MultiArray control_msg;
        control_msg.data.resize(6, 0.0);
        control_pub_->publish(control_msg);
        
        // 发送PWM停止指令
        mavros_msgs::msg::OverrideRCIn stop_pwm_msg;
        std::fill(stop_pwm_msg.channels.begin(), stop_pwm_msg.channels.end(), 65535);
        
        stop_pwm_msg.channels[2] = 1500;  // 电机停止
        stop_pwm_msg.channels[3] = 1500;  // 舵机中立
        
        pwm_pub_->publish(stop_pwm_msg);
        
        // 更新当前PWM值为停止值
        current_motor_pwm_ = 1500;
        current_steer_pwm_ = 1500;
        
        RCLCPP_INFO(this->get_logger(), "Stop commands and PWM stop signals published");
    }

    void setupSignalHandlers()
    {
        // 注册信号处理函数
        signal(SIGINT, signalHandler);   // Ctrl+C
        signal(SIGTERM, signalHandler);  // terminate命令
        signal(SIGQUIT, signalHandler);  // quit信号
        
        RCLCPP_INFO(this->get_logger(), "Signal handlers registered for safe shutdown");
    }
    
    static void signalHandler(int signal)
    {
        if (instance_) {
            RCLCPP_WARN(instance_->get_logger(), 
                "Received signal %d, sending emergency stop PWM...", signal);
            instance_->emergencyStopPWM();
        }
        
        // 继续默认的信号处理
        rclcpp::shutdown();
        exit(signal);
    }
    
    void emergencyStopPWM()
    {
        if (pwm_pub_) {
            try {
                mavros_msgs::msg::OverrideRCIn emergency_stop_msg;
                std::fill(emergency_stop_msg.channels.begin(), emergency_stop_msg.channels.end(), 65535);
                
                emergency_stop_msg.channels[2] = 1500;  // 电机停止
                emergency_stop_msg.channels[3] = 1500;  // 舵机中立
                
                // 连续发送几次确保可靠性
                for (int i = 0; i < 3; i++) {
                    pwm_pub_->publish(emergency_stop_msg);
                    rclcpp::sleep_for(std::chrono::milliseconds(50));
                }
                
                RCLCPP_WARN(this->get_logger(), "Emergency stop PWM signals sent successfully");
                
                // 更新当前PWM状态
                current_motor_pwm_ = 1500;
                current_steer_pwm_ = 1500;
                
            } catch (const std::exception& e) {
                RCLCPP_ERROR(this->get_logger(), "Failed to send emergency stop PWM: %s", e.what());
            }
        } else {
            RCLCPP_ERROR(this->get_logger(), "PWM publisher not available for emergency stop");
        }
    }

    // 在构造函数中添加CSV初始化
    void initializeCSVLogging()
    {
        // 从参数获取CSV日志配置
        this->declare_parameter("debug.enable_csv_logging", true);
        this->declare_parameter("debug.csv_output_directory", "/home/ubuntu/ros2_boat_ws");
        this->declare_parameter("debug.csv_filename", "usv_pid_debug.csv");
        this->declare_parameter("debug.csv_auto_create_dir", true);
        
        csv_logging_enabled_ = this->get_parameter("debug.enable_csv_logging").as_bool();
        std::string csv_output_dir = this->get_parameter("debug.csv_output_directory").as_string();
        std::string csv_base_filename = this->get_parameter("debug.csv_filename").as_string();
        bool auto_create_dir = this->get_parameter("debug.csv_auto_create_dir").as_bool();
        csv_log_counter_ = 0;
        
        if (csv_logging_enabled_) {
            // 确保目录路径以斜杠结尾
            if (!csv_output_dir.empty() && csv_output_dir.back() != '/') {
                csv_output_dir += '/';
            }
            
            // 检查并创建目录
            if (auto_create_dir) {
                // 使用系统调用创建目录（包括父目录）
                std::string mkdir_command = "mkdir -p \"" + csv_output_dir + "\"";
                int result = system(mkdir_command.c_str());
                if (result != 0) {
                    RCLCPP_WARN(this->get_logger(), "Failed to create directory: %s", csv_output_dir.c_str());
                }
            }
            
            // 检查目录是否存在和可写
            if (!directoryExists(csv_output_dir)) {
                RCLCPP_ERROR(this->get_logger(), "CSV output directory does not exist: %s", csv_output_dir.c_str());
                csv_logging_enabled_ = false;
                return;
            }
            
            // 生成带时间戳的完整文件路径
            auto now = std::chrono::system_clock::now();
            auto time_t = std::chrono::system_clock::to_time_t(now);
            auto tm = *std::localtime(&time_t);
            
            std::stringstream ss;
            size_t dot_pos = csv_base_filename.find_last_of('.');
            std::string base_name, extension;
            
            if (dot_pos != std::string::npos) {
                base_name = csv_base_filename.substr(0, dot_pos);
                extension = csv_base_filename.substr(dot_pos);
            } else {
                base_name = csv_base_filename;
                extension = ".csv";
            }
            
            ss << csv_output_dir << base_name
            << "_" << std::put_time(&tm, "%Y%m%d_%H%M%S") 
            << extension;
            csv_filename_ = ss.str();
            
            // 尝试打开文件
            csv_file_.open(csv_filename_);
            if (csv_file_.is_open()) {
                // 写入CSV头部
                csv_file_ << "timestamp,control_cycle,"
                        << "current_lat,current_lon,current_heading,current_speed,"
                        << "target_heading,target_speed,distance_to_target,"
                        << "heading_error,heading_p,heading_i,heading_d,heading_output,"
                        << "speed_error,speed_p,speed_i,speed_d,speed_output,"
                        << "motor_l,motor_r,steer_l,steer_r,"
                        << "waypoint_index,total_waypoints,mission_active\n";
                
                RCLCPP_INFO(this->get_logger(), "CSV logging enabled: %s", csv_filename_.c_str());
            } else {
                RCLCPP_ERROR(this->get_logger(), "Failed to open CSV file: %s", csv_filename_.c_str());
                RCLCPP_ERROR(this->get_logger(), "Please check directory permissions and disk space");
                csv_logging_enabled_ = false;
            }
        } else {
            RCLCPP_INFO(this->get_logger(), "CSV logging disabled");
        }
    }

    // 辅助函数：检查目录是否存在
    bool directoryExists(const std::string& path) 
    {
        struct stat info;
        if (stat(path.c_str(), &info) != 0) {
            return false;  // 无法访问路径
        }
        return (info.st_mode & S_IFDIR) != 0;  // 检查是否为目录
    }

    // 新增：CSV数据记录函数
    void logCSVData(const actuator_control& control_output)
    {
        if (!csv_logging_enabled_ || !csv_file_.is_open()) {
            return;
        }
        
        csv_log_counter_++;
        
        // 获取当前时间戳
        auto now = std::chrono::system_clock::now();
        auto duration = now.time_since_epoch();
        auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
        
        // 计算当前状态
        float current_speed = sqrtf(trans_state_.VelEarth[0] * trans_state_.VelEarth[0] + 
                                trans_state_.VelEarth[1] * trans_state_.VelEarth[1]);
        float current_heading = static_cast<float>(trans_state_.PosEarth[3]);
        float target_heading = motion_planner_->actionHeadvelControl.heading;
        float target_speed = motion_planner_->actionHeadvelControl.velocity;
        
        // 计算误差
        float heading_error = calculateHeadingAngleError(target_heading, current_heading);
        float speed_error = target_speed - current_speed;
        
        // 获取距离信息
        float distance_to_target = motion_planner_->GetDistanceToTarget(trans_state_);
        
        // 获取PID组件
        float heading_p = 0.0f, heading_i = 0.0f, heading_d = 0.0f, heading_output = 0.0f;
        float speed_p = 0.0f, speed_i = 0.0f, speed_d = 0.0f, speed_output = 0.0f;
        
        if (motion_controller_->IsHeadingControlActive()) {
            heading_p = motion_controller_->GetHeadingProportional();
            heading_i = motion_controller_->GetHeadingIntegral();
            heading_d = motion_controller_->GetHeadingDerivative();
            heading_output = motion_controller_->GetLastHeadingOutput();
        }
        
        if (motion_controller_->IsVelocityControlActive()) {
            speed_p = motion_controller_->GetVelocityProportional();
            speed_i = motion_controller_->GetVelocityIntegral();
            speed_d = motion_controller_->GetVelocityDerivative();
            speed_output = motion_controller_->GetLastVelocityOutput();
        }
        
        // 写入CSV数据
        csv_file_ << millis << "," << csv_log_counter_ << ","
                << std::fixed << std::setprecision(6)
                << trans_state_.PosEarth[0] << "," << trans_state_.PosEarth[1] << ","
                << std::setprecision(2)
                << current_heading << "," << current_speed << ","
                << target_heading << "," << target_speed << "," << distance_to_target << ","
                << heading_error << "," << heading_p << "," << heading_i << "," << heading_d << "," << heading_output << ","
                << speed_error << "," << speed_p << "," << speed_i << "," << speed_d << "," << speed_output << ","
                << control_output.motor_l << "," << control_output.motor_r << ","
                << control_output.steer_l << "," << control_output.steer_r << ","
                << motion_planner_->GetCurrentWaypointIndex() << ","
                << motion_planner_->GetTotalWaypoints() << ","
                << (mission_active_ ? 1 : 0) << "\n";
        
        // 定期刷新文件
        if (csv_log_counter_ % 10 == 0) {
            csv_file_.flush();
        }
    }
    
    // 辅助函数：计算航向误差
    float calculateHeadingAngleError(float target, float current)
    {
        float error = target - current;
        while (error > 180.0f) error -= 360.0f;
        while (error < -180.0f) error += 360.0f;
        return error;
    }

    // 参数结构体
    struct TrackingParams {
        double eta;
        double switch_distance;
        double max_speed;
        double default_speed;
        double min_speed_factor;
    };
    
    struct PIDParams {
        double kp, ki, kd;
        double max_integral;
        double min_output, max_output;
    };
    
    struct SafetyParams {
        double max_heading_error;
        double speed_reduction_angle;
        double approach_distance;
    };
    
    // 新增：飞控数据结构体
    struct VelocityData {
        double linear_x, linear_y, linear_z;
        double angular_x, angular_y, angular_z;
    };
    
    struct IMUData {
        double orientation_x, orientation_y, orientation_z, orientation_w;
        double roll, pitch, yaw;
        double linear_acceleration_x, linear_acceleration_y, linear_acceleration_z;
        double angular_velocity_x, angular_velocity_y, angular_velocity_z;
    };
    
    struct GPSData {
        double latitude, longitude, altitude;
    };
    
    // 成员变量
    int usv_type_;
    int usv_id_;
    double control_frequency_;
    bool state_received_ = false;
    bool mission_active_ = false;
    bool debug_enabled_ = false;
    int state_count_ = 0;
    int control_count_ = 0;
    int state_print_interval_;
    int control_print_interval_;
    
    // 新增：飞控数据接收状态标志
    bool velocity_data_received_;
    bool imu_data_received_;
    bool gps_data_received_;
    
    // 新增：飞控数据存储
    VelocityData velocity_data_;
    IMUData imu_data_;
    GPSData gps_data_;
    
    TrackingParams tracking_params_;
    PIDParams heading_pid_params_;
    PIDParams velocity_pid_params_;
    SafetyParams safety_params_;
    
    std::string topic_target_position_;
    std::string topic_usv_state_;
    std::string topic_usv_control_;
    std::string topic_usv_task_; 
    
    kinematic_state current_state_;
    TransState trans_state_;
    mission_control mission_data_;
    
    std::unique_ptr<MotionPlan> motion_planner_;
    std::unique_ptr<MotionControl> motion_controller_;

    // 添加静态成员变量和信号处理函数
    static USVController* instance_;
    
    rclcpp::Time last_control_time_;

    // PWM平滑控制相关
    uint16_t current_motor_pwm_ = 1500;
    uint16_t current_steer_pwm_ = 1500;
    double cycle_time_ms = 500.0;
    int pwm_smooth_steps_ = 5;              // 每个控制周期内的平滑步数
    int step_delay_microseconds_ = 85000;   // 每步之间的延时（微秒）
    
    // 舵机控制优化
    float steering_deadzone_ = 0.5f;
    float steering_filter_alpha_ = 0.4f;
    float last_steering_command_ = 0.0f;
    
    // 控制稳定性
    rclcpp::Time last_significant_change_time_;
    float control_stability_threshold_ = 1.0f; // 1度的稳定性阈值

    // CSV日志相关
    std::ofstream csv_file_;
    bool csv_logging_enabled_;
    std::string csv_filename_;
    int csv_log_counter_;

    // ROS2 接口
    rclcpp::Subscription<custom_msgs::msg::GpsDataArray>::SharedPtr target_pos_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr state_sub_;
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr control_pub_;
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr task_pub_;
    rclcpp::Publisher<mavros_msgs::msg::OverrideRCIn>::SharedPtr pwm_pub_;
    rclcpp::TimerBase::SharedPtr control_timer_;
    
    // 新增：飞控数据订阅者和发布者
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_;
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr usv_state_pub_;
};

USVController* USVController::instance_ = nullptr;

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    
    try {
        auto node = std::make_shared<USVController>();
        RCLCPP_INFO(rclcpp::get_logger("usv_controller"), "USV Controller node started");
        rclcpp::spin(node);
    } catch (const std::exception& e) {
        RCLCPP_ERROR(rclcpp::get_logger("usv_controller"), "Exception: %s", e.what());
        return 1;
    }
    
    RCLCPP_INFO(rclcpp::get_logger("usv_controller"), "USV Controller node shutting down");
    rclcpp::shutdown();
    return 0;
}